import time
import numpy as np

from mcp.server.fastmcp import FastMCP
import json
import logging
from logging.handlers import RotatingFileHandler
from pathlib import Path
import paramiko
import URBasic
from .config import ConnectionConfig, MotionConfig, LogConfig
from .exceptions import (
    NURMCPException, ConnectionException, RobotCommandException, 
    MotionException, ValidationException,
    exception_handler, motion_exception_handler, create_error_response)

# Configure logging
log_dir = Path("logs")
log_dir.mkdir(parents=True, exist_ok=True)
handler = RotatingFileHandler(
    log_dir / "server.log",
    maxBytes=1024 * 1024,  # 1MB
    backupCount=3,  # 保留 3 个旧日志
    encoding="utf-8"
)
formatter = logging.Formatter("%(asctime)s - %(name)s - %(levelname)s - %(message)s")
handler.setFormatter(formatter)

logger = logging.getLogger()
logger.addHandler(handler)
logger.setLevel(logging.INFO)

mcp = FastMCP(
    "nUR_MCP_SERVER",
    description="Control UR robots through the Model Context Protocol"
)

robot_list = None
robotModle_list = None


@exception_handler
def set_robot_list():
    """初始化全局机器人列表。
    
    创建并初始化两个全局字典：
    - robot_list: 存储机器人对象的字典，键为IP地址
    - robotModle_list: 存储机器人模型的字典，键为IP地址
    
    这两个字典将用于在整个应用程序中跟踪和管理机器人连接。
    
    Returns:
        dict: 成功时返回{"status": "ok", "message": "列表初始化成功"}
    
    Raises:
        ConnectionException: 如果初始化过程中出现连接相关错误
    """
    try:
        global robot_list, robotModle_list
        logger.info("初始化全局机器人列表")
        robot_list = dict()
        robotModle_list = dict()
        logger.info("机器人列表初始化完成")
        return {"status": "ok", "message": "列表初始化成功"}
    except Exception as e:
        logger.error(f"初始化机器人列表时发生错误: {str(e)}")
        raise ConnectionException(f"初始化机器人列表失败: {str(e)}")


@exception_handler
def link_check(ip):
    """检查与指定IP地址机器人的连接状态并在需要时建立连接。
    
    验证指定IP地址的机器人是否在robot_list中注册并且连接是否有效。
    如果连接不存在或RTDE连接未运行，则尝试通过connect_ur函数重新建立连接。
    
    Args:
        ip (str): 机器人的IP地址
    
    Returns:
        str: 如果连接已存在且有效，返回'连接成功'；
             否则，返回connect_ur函数的执行结果，通常是JSON格式的连接状态消息
    
    Raises:
        ValidationException: 如果IP地址格式无效
    """
    # 验证IP地址
    import ipaddress
    try:
        ipaddress.ip_address(ip)
    except ValueError:
        raise ValidationException(f"无效的IP地址格式: {ip}", param_name="ip", param_value=ip)
    
    if robot_list.get(ip, "unknown") == "unknown" or not robot_list[
        ip].robotConnector.RTDE.isRunning():
        return connect_ur(ip)
    return '连接成功'


def return_msg(txt: str):
    """将文本消息转换为格式化的JSON字符串。
    
    将输入的文本消息转换为JSON格式，确保中文等非ASCII字符正确显示，并
    添加适当的缩进以提高可读性。
    
    Args:
        txt (str): 要转换为JSON的文本消息
    
    Returns:
        str: 格式化的JSON字符串，包含原始消息，使用缩进并支持非ASCII字符
    """
    return json.dumps(txt, indent=2, ensure_ascii=False)


def right_pose_joint(current_pose, q, tolerance=None):
    """验证机器人关节角度是否在允许的误差范围内。
    
    使用numpy向量化操作高效地检查所有关节角度是否在指定误差范围内匹配。
    
    Args:
        current_pose (list): 当前关节角度列表，长度为6
        q (list): 目标关节角度列表，长度为6
        tolerance (float, optional): 允许的误差范围（弧度）。
            如果未提供，则使用配置文件中的默认值。
    
    Returns:
        bool: 如果所有关节角度都在误差范围内，返回True，否则返回False
    
    Raises:
        ValueError: 如果输入的列表长度不正确
    """
    try:
        # 使用提供的tolerance或从配置文件获取默认值
        actual_tolerance = tolerance if tolerance is not None else MotionConfig.JOINT_TOLERANCE
        
        # 转换为numpy数组以进行向量化操作
        current = np.array(current_pose)
        target = np.array(q)
        
        # 验证输入数组长度
        if len(current) != 6 or len(target) != 6:
            raise ValueError("输入必须是长度为6的列表")
        
        # 使用numpy进行向量化比较，检查所有关节是否在误差范围内
        return np.all(np.abs(current - target) <= actual_tolerance)
    except ValueError as e:
        logger.error(f"关节角度验证输入错误: {str(e)}")
        raise
    except Exception as e:
        logger.error(f"关节角度验证过程中发生错误: {str(e)}")
        # 出错时返回False，表示不匹配
        return False


def round_pose(pose):
    """将机器人位姿坐标四舍五入到指定的小数位数。
    
    对6个坐标值（位置和姿态）进行四舍五入，精确到三位小数，
    用于减少输出数据的精度，便于显示和比较。
    
    Args:
        pose (list): 位姿坐标列表，包含6个值 [x, y, z, rx, ry, rz]
    
    Returns:
        list: 四舍五入后的位姿坐标列表
    """
    pose[0] = round(pose[0], 3)
    pose[1] = round(pose[1], 3)
    pose[2] = round(pose[2], 3)
    pose[3] = round(pose[3], 3)
    pose[4] = round(pose[4], 3)
    pose[5] = round(pose[5], 3)
    return pose


@motion_exception_handler
def movejConfirm(ip, q):
    """确认关节移动命令的执行结果。
    
    监控机器人移动过程，检查是否成功到达目标位置，使用配置文件中的参数控制
    验证间隔和超时处理。
    
    Args:
        ip (str): 机器人IP地址
        q (list): 目标关节角度列表
    
    Returns:
        dict: 移动结果字典
            {"status": "success", "code": 1, "message": "移动成功到位"} 或
            {"status": "error", "code": 2, "message": "移动结束但位置不准确"}
    
    Raises:
        KeyError: 如果指定的IP地址未在robot_list中注册
        AttributeError: 如果机器人对象缺少必要的方法或属性
        MotionException: 如果移动验证过程中发生错误或超时
    """
    loop_flg = True
    count = 0
    start_time = time.time()
    
    # 验证输入参数
    if ip not in robot_list:
        raise KeyError(f"机器人IP {ip} 未在连接列表中注册")
    
    if not isinstance(q, list) or len(q) != 6:
        raise ValidationException(
            "目标关节角度必须是长度为6的列表", 
            param_name="q", 
            param_value=q
        )
    
    try:
        while loop_flg:
            # 检查是否超时
            if time.time() - start_time > MotionConfig.POSE_VERIFICATION_TIMEOUT:
                error_msg = f"机器人{ip}位姿验证超时，超过{MotionConfig.POSE_VERIFICATION_TIMEOUT}秒"
                logger.warning(error_msg)
                raise MotionException(
                    error_msg,
                    motion_type="movej",
                    target=q,
                    details={"timeout": MotionConfig.POSE_VERIFICATION_TIMEOUT}
                )
            
            # 使用配置文件中的验证间隔
            time.sleep(MotionConfig.POSE_VERIFICATION_INTERVAL)
            
            current_pose = round_pose(robot_list[ip].get_actual_joint_positions())
            if right_pose_joint(current_pose, q):
                robot_list[ip].robotConnector.DashboardClient.ur_running()
                running = robot_list[ip].robotConnector.DashboardClient.last_respond
                if running == 'Program running: false':
                    logger.info(f"机器人{ip}成功移动到位")
                    return {"status": "success", "code": 1, "message": "移动成功到位"}
            else:
                robot_list[ip].robotConnector.DashboardClient.ur_running()
                running = robot_list[ip].robotConnector.DashboardClient.last_respond

                if running == 'Program running: true':
                    # 尚未移动完成
                    continue
                else:
                    # 移动完成
                    count = count + 1
                    if count > 5:
                        return 2
    except (KeyError, AttributeError) as e:
        logger.error(f"移动确认过程中发生错误: {str(e)}")
        raise


def right_pose_tcp(current_pose_1, pose):
    """tcp位置是否一致的校验，这里允许10mm的误差"""
    if pose[0] + 0.010 >= current_pose_1[0] >= pose[0] - 0.010:
        if pose[1] + 0.010 >= current_pose_1[1] >= pose[1] - 0.010:
            if pose[2] + 0.010 >= current_pose_1[2] >= pose[2] - 0.010:
                return True

    return False


def movelConfirm(ip, pose):
    """
    movel移动的结果确认
    1：移动到位
    2：移动结束，但是位置不准确
    """
    loop_flg = True
    count = 0
    while loop_flg:
        time.sleep(1)
        current_pose = round_pose(robot_list[ip].get_actual_tcp_pose())
        if right_pose_tcp(current_pose, pose):
            robot_list[ip].robotConnector.DashboardClient.ur_running()
            running = robot_list[ip].robotConnector.DashboardClient.last_respond
            if running == 'Program running: false':
                return 1
        else:
            robot_list[ip].robotConnector.DashboardClient.ur_running()
            running = robot_list[ip].robotConnector.DashboardClient.last_respond

            if running == 'Program running: true':
                '''尚未移动完成'''
                continue
            else:
                '''移动完成'''
                count = count + 1
                if count > 5:
                    return 2


@mcp.tool()
def connect_ur(ip: str):
    """根据用户提供的IP连接UR
    IP：机器人地址"""
    try:
        host = ip
        global robot_list, robotModle_list

        if robot_list.get(ip, "unknown") != "unknown":
            robot_list[ip].robotConnector.close()
            return return_msg(f"优傲机器人连接失败: {ip}")

        robotModle = URBasic.robotModel.RobotModel()
        robot = URBasic.urScriptExt.UrScriptExt(host=host, robotModel=robotModle)
        robot_list[ip] = robot
        robotModle_list[ip] = robotModle

        if robot_list.get(ip, "unknown") == "unknown" or not robot_list[
            ip].robotConnector.RTDE.isRunning():
            return return_msg(f"优傲机器人连接失败: {ip}")
        robot_list[ip].robotConnector.DashboardClient.ur_is_remote_control()
        remote = robot_list[ip].robotConnector.DashboardClient.last_respond.lower()
        print(f"remote:{remote}")
        if remote != 'true' and not remote.startswith('could not understand'):
            disconnect_ur(ip)
            return return_msg(f"请检查机器人是否处于远程控制模式。IP:{host}")

        return return_msg(f"连接成功。IP:{host}")
    except Exception as e:
        logger.error(f"优傲机器人连接失败: {str(e)}")
        return return_msg(f"优傲机器人连接失败: {str(e)}")


@mcp.tool()
def disconnect_ur(ip: str):
    """根据用户提供的IP，断开与UR机器人的连接
    IP：机器人地址"""
    try:
        if robot_list.get(ip, "unknown") == "unknown":
            return return_msg("连接不存在")
        robot_list[ip].close()
        return return_msg("连接已断开。")
    except Exception as e:
        logger.error(f"连接断开失败: {str(e)}")
        return return_msg(f"连接断开失败: {str(e)}")


@mcp.tool()
def get_serial_number(ip: str):
    """获取指定IP机器人的序列号
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        robot_list[ip].robotConnector.DashboardClient.ur_serial_number()
        return return_msg(
            f"IP为{ip}的优傲机器人的序列号为： {robot_list[ip].robotConnector.DashboardClient.last_respond}")
    except Exception as e:
        logger.error(f"获取序列号失败: {str(e)}")
        return return_msg(f"获取序列号失败: {str(e)}")


@mcp.tool()
def get_time(ip: str) -> str:
    """根据用户提供的IP，获取指定机器人的开机时长(秒)
    IP：机器人地址"""
    try:
        if '连接成功' not in link_check(ip):
            return return_msg(f"与机器人的连接已断开。IP:{ip}")

        return return_msg(f"{robotModle_list[ip].RobotTimestamp():.2f}")
    except Exception as e:
        logger.error(f"获取开机时长失败: {str(e)}")
        return return_msg(f"获取开机时长失败: {str(e)}")


@mcp.tool()
def get_ur_software_version(ip: str):
    """根据用户提供的IP，获取指定机器人的软件版本
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        robot_list[ip].robotConnector.DashboardClient.ur_polyscopeVersion()
        result = robot_list[ip].robotConnector.DashboardClient.last_respond
        return return_msg(f"IP为{ip}的优傲机器人的软件版本是{result}")
    except Exception as e:
        logger.error(f"软件版本获取失败: {str(e)}")
        return return_msg(f"软件版本获取失败: {str(e)}")


@mcp.tool()
def get_robot_model(ip: str):
    """获取指定IP的机器人型号
    IP：机器人地址"""
    try:
        robot_list[ip].robotConnector.DashboardClient.ur_get_robot_model()
        model = robot_list[ip].robotConnector.DashboardClient.last_respond
        robot_list[ip].robotConnector.DashboardClient.ur_is_remote_control()
        e = robot_list[ip].robotConnector.DashboardClient.last_respond.lower()
        if e == 'true' or e == 'false':
            model = f"{model}e"
        return return_msg(model)
    except Exception as e:
        logger.error(f"获取机器人型号失败: {str(e)}")
        return return_msg(f"获取机器人型号失败: {str(e)}")


@mcp.tool()
def get_safety_mode(ip: str):
    """获取指定IP机器人的安全模式
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        robot_list[ip].robotConnector.DashboardClient.ur_safetymode()
        result = robot_list[ip].robotConnector.DashboardClient.last_respond
        return return_msg(f"IP为{ip}的优傲机器人的安全模式是{result}")
    except Exception as e:
        logger.error(f"安全模式获取失败: {str(e)}")
        return return_msg(f"安全模式获取失败: {str(e)}")


@mcp.tool()
def get_robot_mode(ip: str):
    """获取指定IP机器人的运行状态
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        robot_list[ip].robotConnector.DashboardClient.ur_robotmode()
        return return_msg(
            f"IP为{ip}的优傲机器人的运行状态为： {robot_list[ip].robotConnector.DashboardClient.last_respond}")
    except Exception as e:
        logger.error(f"运行状态获取失败: {str(e)}")
        return return_msg(f"运行状态获取失败: {str(e)}")


@mcp.tool()
def get_program_state(ip: str):
    """获取指定IP机器人的程序执行状态
        IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")

        robot_list[ip].robotConnector.DashboardClient.ur_get_loaded_program()
        prog_name = robot_list[ip].robotConnector.DashboardClient.last_respond
        robot_list[ip].robotConnector.DashboardClient.ur_programState()
        prog_state = robot_list[ip].robotConnector.DashboardClient.last_respond
        robot_list[ip].robotConnector.DashboardClient.ur_isProgramSaved()
        flg = robot_list[ip].robotConnector.DashboardClient.last_respond
        robot_list[ip].robotConnector.DashboardClient.ur_running()
        running = robot_list[ip].robotConnector.DashboardClient.last_respond

        prog_saved = ''
        prog_running = ''
        if flg.startswith("false"):
            prog_saved = '程序未保存，请及时保存或备份正在编辑的程序。'
        if running == 'Program running: true':
            prog_running = '机械臂正在动作。'
        return return_msg(
            f"IP为{ip}的优傲机器人当前加载的程序是：{prog_name}，程序的执行状态是：{prog_state}。{prog_saved}。{prog_running}")
    except Exception as e:
        logger.error(f"程序的执行状态获取失败: {str(e)}")
        return return_msg(f"程序的执行状态获取失败: {str(e)}")


@mcp.tool()
def get_actual_tcp_pose(ip: str):
    """获取指定IP机器人的当前TCP位置
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")

        return return_msg(f"当前TCP位置： {robot_list[ip].get_actual_tcp_pose()}")
    except Exception as e:
        logger.error(f"TCP位置获取失败: {str(e)}")
        return return_msg(f"TCP位置获取失败: {str(e)}")


@mcp.tool()
def get_actual_joint_pose(ip: str):
    """获取指定IP机器人的当前关节角度
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        return return_msg(f"当前关节姿态 {robot_list[ip].get_actual_joint_positions()}")
    except Exception as e:
        logger.error(f"TCP位置获取失败: {str(e)}")
        return return_msg(f"TCP位置获取失败: {str(e)}")


@mcp.tool()
def get_output_int_register(ip: str, index: int):
    """获取指定IP机器人Int寄存器的值,
    IP：机器人地址
    index：寄存器下标，范围是[0，23]"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        return return_msg(f"{robotModle_list[ip].OutputIntRegister(index)}")
    except Exception as e:
        logger.error(f"Int寄存器的值获取失败: {str(e)}")
        return return_msg(f"Int寄存器的值获取失败: {str(e)}")


@mcp.tool()
def get_output_double_register(ip: str, index: int):
    """获取指定IP机器人Double寄存器的值,
    IP：机器人地址
    index：寄存器下标，范围是[0，23]"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        return return_msg(f"{robotModle_list[ip].OutputDoubleRegister(index)}")
    except Exception as e:
        logger.error(f"Double寄存器的值获取失败: {str(e)}")
        return return_msg(f"Double寄存器的值获取失败: {str(e)}")


@mcp.tool()
def get_output_bit_register(ip: str, index: int):
    """获取指定IP机器人Bool寄存器的值,
    IP：机器人地址
    index：寄存器下标，范围是[0，23]"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        bits = robotModle_list[ip].OutputBitRegister()
        return return_msg(f"{bits[index]}")
    except Exception as e:
        logger.error(f"Bool寄存器的值获取失败: {str(e)}")
        return return_msg(f"Bool寄存器的值获取失败: {str(e)}")


@mcp.tool()
def get_actual_robot_voltage(ip: str):
    """获取指定IP机器人的电压（伏特）
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        return return_msg(f"{robotModle_list[ip].ActualRobotVoltage()}（伏特）")
    except Exception as e:
        logger.error(f"机器人的电压获取失败: {str(e)}")
        return return_msg(f"机器人的电压获取失败: {str(e)}")


@mcp.tool()
def get_actual_robot_current(ip: str):
    """获取指定IP机器人的电流（安培）
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        return return_msg(f"{robotModle_list[ip].ActualRobotCurrent()}（安培）")
    except Exception as e:
        logger.error(f"机器人的电流获取失败: {str(e)}")
        return return_msg(f"机器人的电流获取失败: {str(e)}")


@mcp.tool()
def get_actual_joint_voltage(ip: str):
    """获取指定IP机器人的各关节电压（伏特）
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        return return_msg(f"{robotModle_list[ip].ActualJointVoltage()}（伏特）")
    except Exception as e:
        logger.error(f"机器人的关节电压获取失败: {str(e)}")
        return return_msg(f"机器人的关节电压获取失败: {str(e)}")


@mcp.tool()
def get_actual_joint_current(ip: str):
    """获取指定IP机器人各关节的电流（安培）
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        return return_msg(f"{robotModle_list[ip].ActualJointVoltage()}（安培）")
    except Exception as e:
        logger.error(f"机器人各关节的电流获取失败: {str(e)}")
        return return_msg(f"机器人各关节的电流获取失败: {str(e)}")


@mcp.tool()
def get_joint_temperatures(ip: str):
    """获取指定IP机器人各关节的温度（摄氏度）。
    IP：机器人地址"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        return return_msg(f"{robotModle_list[ip].JointTemperatures()}（摄氏度）")
    except Exception as e:
        logger.error(f"机器人各关节的温度获取失败: {str(e)}")
        return return_msg(f"机器人各关节的温度获取失败: {str(e)}")


@mcp.tool()
def get_programs(ip: str, username='root', password='easybot'):
    """获取指定IP机器人的所有程序。
    IP：机器人地址
    username：ssh账号
    password：ssh密码
    """
    try:
        ssh = paramiko.SSHClient()
        ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
        ssh.connect(hostname=ip, port=22, username=username, password=password)
        # 创建交互式 shell
        shell = ssh.invoke_shell()
        # 执行多个命令
        shell.send('cd /programs\n')
        shell.send('ls -1\n')
        # 获取输出
        import time
        time.sleep(1)  # 等待命令执行
        output = shell.recv(65535).decode()
        ssh.close()
        files = []
        for file in output.split('\n'):
            name = file.replace(' ', '').replace('\r', '')
            if name.endswith('.urp'):
                files.append(name)
        return return_msg(f"命令已发送：{str(files)}")
    except Exception as e:
        return return_msg(f"程序列表获取失败。{str(e)}")


@mcp.tool()
def send_program_script(ip: str, script: str):
    """发送脚本到指定IP的机器人。
    IP：机器人地址
    script：脚本内容"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        robot_list[ip].robotConnector.RealTimeClient.SendProgram(script)
        return return_msg(f"脚本程序已发送，请确认执行结果。")
    except Exception as e:
        logger.error(f"发送脚本失败: {str(e)}")
        return return_msg(f"发送脚本失败: {str(e)}")


@mcp.tool()
def movej(ip: str, q: dict, a=1, v=1, t=0, r=0):
    """发送新的关节姿态到指定IP的机器人，使每个关节都旋转至指定弧度。
    IP：机器人地址
    q：各关节角度
    a：加速度（米每平方秒）
    v：速度（米每秒）
    t：移动时长（秒）
    r：交融半径（米）"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")

        cmd = f"movej({q},{a},{v},{t},{r})"
        robot_list[ip].movej(q, a, v, t, r)
        result = movejConfirm(ip, q)
        if result == 1:
            return return_msg(f"命令 {cmd} 已发送，移动完成。")
        else:
            return return_msg(f"命令 {cmd} 已发送，移动失败。")
    except Exception as e:
        logger.error(f"发送新的关节姿态到UR机器人失败: {str(e)}")
        return return_msg(f"发送新的关节姿态到UR机器人: {str(e)}")


@mcp.tool()
def movel(ip: str, pose: dict, a=1, v=1, t=0, r=0):
    """发送新的TCP位置到指定IP的机器人，使TCP移动到指定位置，移动期间TCP作直线移动。
    IP：机器人地址
    pose：TCP位置
    a：加速度（米每平方秒）
    v：速度（米每秒）
    t：移动时长（秒）
    r：交融半径（米）"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")

        robot_list[ip].movel(pose, a, v, t, r)
        result = movelConfirm(ip, pose)
        cmd = f"movel(p{pose},{a},{v},{t},{r})"
        if result == 1:
            return return_msg(f"命令 {cmd} 已发送，移动完成。")
        else:
            return return_msg(f"命令 {cmd} 已发送，移动失败。")
    except Exception as e:
        logger.error(f"发送新的TCP位置到指定IP的机器人失败: {str(e)}")
        return return_msg(f"发送新的TCP位置到指定IP的机器人: {str(e)}")


@mcp.tool()
def movel_x(ip: str, distance: float):
    """命令指定IP机器人的TCP沿X轴方向移动
    IP：机器人地址
    distance：移动距离（米）"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        pose = robot_list[ip].get_actual_tcp_pose()
        pose[0] = pose[0] + distance
        robot_list[ip].movel(pose)
        result = movelConfirm(ip, pose)
        cmd = f"movel(p[{'{:.4f}'.format(pose[0])},{'{:.4f}'.format(pose[1])},{'{:.4f}'.format(pose[2])},{'{:.4f}'.format(pose[3])},{'{:.4f}'.format(pose[4])},{'{:.4f}'.format(pose[5])},],0.5,0.25,0,0)"
        if result == 1:
            return return_msg(f"命令 {cmd} 已发送，移动完成。")
        else:
            return return_msg(f"命令 {cmd} 已发送，移动失败。")
    except Exception as e:
        logger.error(f"移动失败: {str(e)}")
        return return_msg(f"移动失败: {str(e)}")


@mcp.tool()
def movel_y(ip: str, distance: float):
    """命令指定IP机器人的TCP沿Y轴方向移动
    IP：机器人地址
    distance：移动距离（米）"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        pose = robot_list[ip].get_actual_tcp_pose()
        pose[1] = pose[1] + distance

        robot_list[ip].movel(pose)
        result = movelConfirm(ip, pose)
        cmd = f"movel(p[{'{:.4f}'.format(pose[0])},{'{:.4f}'.format(pose[1])},{'{:.4f}'.format(pose[2])},{'{:.4f}'.format(pose[3])},{'{:.4f}'.format(pose[4])},{'{:.4f}'.format(pose[5])},],0.5,0.25,0,0)"
        if result == 1:
            return return_msg(f"命令 {cmd} 已发送，移动完成。")
        else:
            return return_msg(f"命令 {cmd} 已发送，移动失败。")
    except Exception as e:
        logger.error(f"移动失败: {str(e)}")
        return return_msg(f"移动失败: {str(e)}")


@mcp.tool()
def movel_z(ip: str, distance: float):
    """命令指定IP机器人的TCP沿Y轴方向移动
    IP：机器人地址
    distance：移动距离（米）"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        pose = robot_list[ip].get_actual_tcp_pose()
        pose[2] = pose[2] + distance
        robot_list[ip].movel(pose)
        result = movelConfirm(ip, pose)
        cmd = f"movel(p[{'{:.4f}'.format(pose[0])},{'{:.4f}'.format(pose[1])},{'{:.4f}'.format(pose[2])},{'{:.4f}'.format(pose[3])},{'{:.4f}'.format(pose[4])},{'{:.4f}'.format(pose[5])},],0.5,0.25,0,0)"
        if result == 1:
            return return_msg(f"命令 {cmd} 已发送，移动完成。")
        else:
            return return_msg(f"命令 {cmd} 已发送，移动失败。")
    except Exception as e:
        logger.error(f"移动失败: {str(e)}")
        return return_msg(f"移动失败: {str(e)}")


@mcp.tool()
def draw_circle(ip: str, center: dict, r: float, coordinate="z"):
    """命令指定IP的机器人，给定圆心位置和半径，在水平或竖直方向画一个圆
        center：圆心的TCP位置
        r：半径（米）
        coordinate：圆所在的平面。z：圆形所在的平面与基座所在平面垂直,其它：圆形所在的平面与基座所在平面平行。默认值：z。"""
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        wp_1 = [center[0], center[1], center[2], center[3], center[4], center[5]]
        wp_2 = [center[0], center[1], center[2], center[3], center[4], center[5]]
        wp_3 = [center[0], center[1], center[2], center[3], center[4], center[5]]
        wp_4 = [center[0], center[1], center[2], center[3], center[4], center[5]]
        cmd = ''
        if coordinate.lower() == "z":
            wp_1[2] = wp_1[2] + r

            wp_2[1] = wp_2[1] + r

            wp_3[2] = wp_3[2] - r

            wp_4[1] = wp_4[1] - r
        else:
            wp_1[0] = wp_1[0] - r

            wp_2[1] = wp_2[1] + r

            wp_3[0] = wp_3[0] + r

            wp_4[1] = wp_4[1] - r

        cmd = (f"movep(p{str(wp_1)}, a=1, v=0.25, r=0.025)\nmovec(p{str(wp_2)}, p{str(wp_3)}, a=1, v=0.25, "
               f"r=0.025, mode=0)\nmovec(p{str(wp_4)}, p{str(wp_1)}, a=1, v=0.25, r=0.025, mode=0)")

        robot_list[ip].robotConnector.RealTimeClient.SendProgram(cmd)
        return return_msg(f"命令已发送：{cmd}")
    except Exception as e:
        logger.error(f"命令发送失败: {str(e)}")
        return return_msg(f"命令发送失败: {str(e)}")


@mcp.tool()
def draw_square(ip: str, origin: dict, border: float, coordinate="z"):
    """给定起点位置和边长，在水平或竖直方向画一个正方形
        origin：画正方形时TCP的起点位置
        border：边长（米）
        coordinate：圆所在的平面。z：圆形所在的平面与基座所在平面垂直,其它：圆形所在的平面与基座所在平面平行。默认值：z。
        """
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        wp_1 = [origin[0], origin[1], origin[2], origin[3], origin[4], origin[5]]
        wp_2 = [origin[0], origin[1], origin[2], origin[3], origin[4], origin[5]]
        wp_3 = [origin[0], origin[1], origin[2], origin[3], origin[4], origin[5]]
        if coordinate.lower() == "z":
            wp_1[1] = wp_1[1] + border

            wp_2[1] = wp_2[1] + border
            wp_2[3] = wp_2[3] - border

            wp_3[3] = wp_3[3] - border

        else:
            wp_1[1] = wp_1[1] + border

            wp_2[1] = wp_2[1] + border
            wp_2[0] = wp_2[0] + border

            wp_3[0] = wp_3[0] + border

        cmd = (f"movel(p{str(origin)}, a=1, v=0.25)\nmovel(p{str(wp_1)}, a=1, v=0.25)\n"
               f"movel(p{str(wp_2)}, a=1, v=0.25)\nmovel(p{str(wp_3)}, a=1, v=0.25)\n"
               f"movel(p{str(origin)}, a=1, v=0.25)")
        robot_list[ip].robotConnector.RealTimeClient.SendProgram(cmd)
        return return_msg(f"命令已发送：{cmd}")
    except Exception as e:
        logger.error(f"命令发送失败: {str(e)}")
        return return_msg(f"命令发送失败: {str(e)}")


@mcp.tool()
def draw_rectangle(ip: str, origin: dict, width: float, height: float, coordinate="z"):
    """给定起点位置和边长，在水平或竖直方向画一个正方形
            origin：画长方形时TCP的起点位置
            width：长（米）
            height：宽（米）
            coordinate：圆所在的平面。z：圆形所在的平面与基座所在平面垂直,其它：圆形所在的平面与基座所在平面平行。默认值：z。"""

    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        wp_1 = [origin[0], origin[1], origin[2], origin[3], origin[4], origin[5]]
        wp_2 = [origin[0], origin[1], origin[2], origin[3], origin[4], origin[5]]
        wp_3 = [origin[0], origin[1], origin[2], origin[3], origin[4], origin[5]]
        if coordinate.lower() == "z":
            wp_1[1] = wp_1[1] + width

            wp_2[1] = wp_2[1] + width
            wp_2[3] = wp_2[3] - height

            wp_3[3] = wp_3[3] - height

        else:
            wp_1[1] = wp_1[1] + width

            wp_2[1] = wp_2[1] + width
            wp_2[0] = wp_2[0] + height

            wp_3[0] = wp_3[0] + height

        cmd = (f"movel(p{str(origin)}, a=1, v=0.25)\nmovel(p{str(wp_1)}, a=1, v=0.25)\n"
               f"movel(p{str(wp_2)}, a=1, v=0.25)\nmovel(p{str(wp_3)}, a=1, v=0.25)\n"
               f"movel(p{str(origin)}, a=1, v=0.25)")
        robot_list[ip].robotConnector.RealTimeClient.SendProgram(cmd)
        return return_msg(f"命令已发送：{cmd}")
    except Exception as e:
        logger.error(f"命令发送失败: {str(e)}")
        return return_msg(f"命令发送失败: {str(e)}")


# Main execution

def main():
    """Run the MCP server"""
    logger.info("Nonead-Universal-Robots-MCP  启动")
    set_robot_list()
    mcp.run()


if __name__ == "__main__":
    main()
